{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "stereocamera-title"
      },
      "source": [
        "# StereoCamera"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "license_cell",
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "source": [
        "GTSAM Copyright 2010-2022, Georgia Tech Research Corporation,\nAtlanta, Georgia 30332-0415\nAll Rights Reserved\n\nAuthors: Frank Dellaert, et al. (see THANKS for the full author list)\n\nSee LICENSE for the license information"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "stereocamera-colab-badge"
      },
      "source": [
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/geometry/doc/StereoCamera.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "stereocamera-intro"
      },
      "source": [
        "A `StereoCamera` in GTSAM models a calibrated stereo camera rig composed of a [`Pose3`](./Pose3.ipynb) representing the left camera's pose in the world frame and a [`Cal3_S2Stereo`](./Cal3_S2Stereo.ipynb) object that contains the intrinsic parameters of both cameras and the baseline between them.\n",
        "\n",
        "It has two main functionalities:\n",
        "- Project a 3D point from the world frame into stereo image coordinates $(u_L, u_R, v)$, represented by a [`StereoPoint2`](./StereoPoint2.ipynb)\n",
        "- Backproject a stereo measurement $(u_L, u_R, v)$ into a 3D point in the world frame, using the known pose and calibration of the camera.\n",
        "\n",
        "In simulation, `StereoCamera.project()` allows you to generate synthetic measurements from known 3D points, which is useful for testing and verifying SLAM or structure-from-motion algorithms.\n",
        "\n",
        "In real-world applications, stereo image pairs are used to extract matching pixel coordinates $(u_L, u_R, v)$, which can then be passed to `StereoCamera.backproject()` to recover 3D points based on the pose of the camera.\n",
        "\n",
        "To see how `StereoCamera` is used in practical applications, please refer to [StereoVOExample](../../../python/gtsam/examples/StereoVOExample.ipynb) and [StereoVOExample_large](../../../python/gtsam/examples/StereoVOExample_large.ipynb)."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 1,
      "metadata": {
        "id": "stereocamera-pip-install",
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "try:\n",
        "    import google.colab\n",
        "    %pip install --quiet gtsam-develop\n",
        "except ImportError:\n",
        "    pass"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 2,
      "metadata": {
        "id": "stereocamera-imports",
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "import gtsam\n",
        "import numpy as np"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "stereocamera-initialization-header"
      },
      "source": [
        "## Initialization\n",
        "\n",
        "A `StereoCamera` can be initialized in two ways:"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 3,
      "metadata": {
        "id": "stereocamera-initialization-code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Default constructor:\n",
            ".camera. R: [\n",
            "\t1, 0, 0;\n",
            "\t0, 1, 0;\n",
            "\t0, 0, 1\n",
            "]\n",
            "t: 0 0 0\n",
            ".calibration. K: 1 0 0\n",
            "0 1 0\n",
            "0 0 1\n",
            "Baseline: 1\n",
            "\n",
            "From left-camera pose and shared calibration K:\n",
            ".camera. R: [\n",
            "\t6.12323e-17, 6.12323e-17, 1;\n",
            "\t-1, 3.7494e-33, 6.12323e-17;\n",
            "\t-0, -1, 6.12323e-17\n",
            "]\n",
            "t: 0 0 5\n",
            ".calibration. K: 1000    0  640\n",
            "   0 1000  360\n",
            "   0    0    1\n",
            "Baseline: 0.5\n",
            "\n"
          ]
        }
      ],
      "source": [
        "# Default constructor\n",
        "stereo_camera_default = gtsam.StereoCamera()\n",
        "print(\"Default constructor:\")\n",
        "print(stereo_camera_default)\n",
        "\n",
        "# With left-camera pose and shared calibration\n",
        "left_camera_pose = gtsam.Pose3(gtsam.Rot3.Ypr(-np.pi/2, 0, -np.pi/2), gtsam.Point3(0, 0, 5.0))\n",
        "fx, fy, s, u0, v0, b = 1000, 1000, 0, 640, 360, 0.5\n",
        "K = gtsam.Cal3_S2Stereo(fx, fy, s, u0, v0, b)\n",
        "stereo_camera = gtsam.StereoCamera(left_camera_pose, K)\n",
        "print(\"From left-camera pose and shared calibration K:\")\n",
        "print(stereo_camera)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Properties"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "`StereoCamera` properties can be accessed by the following member functions:\n",
        "- `pose()`: Returns a [`Pose3`](./Pose3.ipynb) object representing the pose of the left camera in the world frame.\n",
        "- `calibration()`: Returns a [`Cal3_S2Stereo`](./Cal3_S2Stereo.ipynb) object, which includes the instrinsic parameters shared by both cameras.\n",
        "- `baseline()`: Returns the baseline, the distance between left and right cameras."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 4,
      "metadata": {
        "id": "stereocamera-properties-code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Stereo camera properties:\n",
            "Camera pose: R: [\n",
            "\t6.12323e-17, 6.12323e-17, 1;\n",
            "\t-1, 3.7494e-33, 6.12323e-17;\n",
            "\t-0, -1, 6.12323e-17\n",
            "]\n",
            "t: 0 0 5\n",
            "\n",
            "Calibration: K: 1000    0  640\n",
            "   0 1000  360\n",
            "   0    0    1\n",
            "Baseline: 0.5\n",
            "\n",
            "Baseline: 0.5\n"
          ]
        }
      ],
      "source": [
        "camera_pose = stereo_camera.pose()\n",
        "calibration = stereo_camera.calibration()\n",
        "baseline = stereo_camera.baseline()\n",
        "\n",
        "print(\"Stereo camera properties:\")\n",
        "print(f\"Camera pose: {stereo_camera.pose()}\")\n",
        "print(f\"Calibration: {stereo_camera.calibration()}\")\n",
        "print(f\"Baseline: {stereo_camera.baseline()}\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Basic Operations\n",
        "\n",
        "### `project()`\n",
        "\n",
        "The `project()` member function maps a 3D point in the world frame to 2D stereo image coordinates. It takes a `Point3` and returns a [`StereoPoint2`](./StereoPoint2.ipynb) containing the pixel coordinates $(u_L, u_R, v)$ observed by the left and right cameras.\n",
        "\n",
        "This function is especially useful in simulation, where the 3D location of a point is known in advance. It enables the projection of ground-truth landmarks into pixel measurements based on the camera's pose and calibration.\n",
        "\n",
        "If the point lies behind the camera or is not visible in the stereo field of view, a `StereoCheiralityException` is thrown."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 5,
      "metadata": {
        "id": "stereocamera-project-code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "3D Point in world frame: [5. 3. 2.]\n",
            "Projected StereoPoint2 (uL, uR, v): (40, -60, 960)\n",
            "\n"
          ]
        }
      ],
      "source": [
        "p_world = gtsam.Point3(5, 3, 2)\n",
        "p_stereo = stereo_camera.project(p_world)\n",
        "\n",
        "print(f\"3D Point in world frame: {p_world}\")\n",
        "print(f\"Projected StereoPoint2 (uL, uR, v): {p_stereo}\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "### `project2()`\n",
        "\n",
        "The `project2()` member function is the same as `project()`, except that it can also calculate the Jacobians of the projection with respect to the camera pose (`H1`) and the 3D point (`H2`)."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 6,
      "metadata": {
        "id": "stereocamera-project-jacobian-code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "3D Point in world frame: [5. 3. 2.]\n",
            "Projected StereoPoint2 (uL, uR, v): (40, -60, 960)\n",
            "\n",
            "Jacobian w.r.t. Pose (H1):\n",
            " [[ -360. -1360.   600.  -200.     0.  -120.]\n",
            " [ -420. -1420.   600.  -200.     0.  -140.]\n",
            " [ 1360.   360.   600.     0.  -200.   120.]]\n",
            "\n",
            "Jacobian w.r.t. Point (H2):\n",
            " [[ 1.20000000e+02 -2.00000000e+02  7.34788079e-15]\n",
            " [ 1.40000000e+02 -2.00000000e+02  8.57252759e-15]\n",
            " [-1.20000000e+02 -7.34788079e-15 -2.00000000e+02]]\n"
          ]
        }
      ],
      "source": [
        "H1 = np.zeros((3, 6), order='F') # Jacobian w.r.t. camera pose\n",
        "H2 = np.zeros((3, 3), order='F') # Jacobian w.r.t. point\n",
        "\n",
        "p_stereo = stereo_camera.project2(p_world, H1, H2)\n",
        "\n",
        "print(f\"3D Point in world frame: {p_world}\")\n",
        "print(f\"Projected StereoPoint2 (uL, uR, v): {p_stereo}\")\n",
        "print(\"Jacobian w.r.t. Pose (H1):\\n\", H1)\n",
        "print(\"\\nJacobian w.r.t. Point (H2):\\n\", H2)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "### `backproject`\n",
        "\n",
        "The `backproject()` member function performs the inverse operation of `project()`. Given a [`StereoPoint2`](./StereoPoint2) measurement $(u_L, u_R, v)$, it reconstructs and returns the corresponding 3D point in the world frame as a `Point3`.\n",
        "\n",
        "This function is especially useful in real-world applications, where stereo image pairs are used to extract pixel correspondences. When the stereo rig's pose and calibration are known (which should be true if you are using a `StereoCamera` in the first place), `backproject()` allows recovering the estimated position of a point in space from its stereo measurement."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 7,
      "metadata": {
        "id": "stereocamera-backproject-code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Original Point: [5. 3. 2.]\n",
            "Reprojected Point: [5. 3. 2.]\n"
          ]
        }
      ],
      "source": [
        "reprojected_point = stereo_camera.backproject(p_stereo)\n",
        "\n",
        "print(f\"Original Point: {p_world}\")\n",
        "print(f\"Reprojected Point: {reprojected_point}\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "### `backproject2()`\n",
        "\n",
        "The `backproject2()` member function is the same as `backproject()`, except that it can also calculater the Jacobians of the backprojection with respect to the stereo measurement (`H1`) and the camera pose (`H2`)."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 8,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Original Point: [5. 3. 2.]\n",
            "Reprojected Point: [5. 3. 2.]\n",
            "\n",
            "Jacobian w.r.t. Pose (H1):\n",
            " [[ 3.00000000e+00  3.00000000e+00 -3.67394040e-16  6.12323400e-17\n",
            "   6.12323400e-17  1.00000000e+00]\n",
            " [ 1.83697020e-16 -5.00000000e+00  3.00000000e+00 -1.00000000e+00\n",
            "   3.74939946e-33  6.12323400e-17]\n",
            " [ 5.00000000e+00  1.83697020e-16  3.00000000e+00 -0.00000000e+00\n",
            "  -1.00000000e+00  6.12323400e-17]]\n",
            "\n",
            "Jacobian w.r.t. Point (H2):\n",
            " [[-5.00000000e-02  5.00000000e-02  3.06161700e-19]\n",
            " [-3.50000000e-02  3.00000000e-02  1.87469973e-35]\n",
            " [ 3.00000000e-02 -3.00000000e-02 -5.00000000e-03]]\n"
          ]
        }
      ],
      "source": [
        "H1 = np.zeros((3, 6), order='F') # Jacobian w.r.t. camera pose\n",
        "H2 = np.zeros((3, 3), order='F') # Jacobian w.r.t. point\n",
        "\n",
        "reprojected_point = stereo_camera.backproject2(p_stereo, H1, H2)\n",
        "\n",
        "print(f\"Original Point: {p_world}\")\n",
        "print(f\"Reprojected Point: {reprojected_point}\")\n",
        "print(\"\\nJacobian w.r.t. Pose (H1):\\n\", H1)\n",
        "print(\"\\nJacobian w.r.t. Point (H2):\\n\", H2)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "stereocamera-manifold-header"
      },
      "source": [
        "## Manifold Operations\n",
        "\n",
        "`StereoCamera` is a manifold, meaning it supports operations such as `retract()` and `localCoordinates()` that defines a continuous space of camera poses. These operations apply to the `Pose3` component (the left camera's pose), while the stereo calibration remains fixed.\n",
        "\n",
        "This structure allows `StereoCamera` to be integrated into optimization routines where small updates to the camera pose are computed on the manifold."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 9,
      "metadata": {
        "id": "stereocamera-manifold-code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Original StereoCamera:\n",
            ".camera. R: [\n",
            "\t6.12323e-17, 6.12323e-17, 1;\n",
            "\t-1, 3.7494e-33, 6.12323e-17;\n",
            "\t-0, -1, 6.12323e-17\n",
            "]\n",
            "t: 0 0 5\n",
            ".calibration. K: 1000    0  640\n",
            "   0 1000  360\n",
            "   0    0    1\n",
            "Baseline: 0.5\n",
            "\n",
            "Retracted StereoCamera:\n",
            ".camera. R: [\n",
            "\t-0.18054, 0.127335, 0.97529;\n",
            "\t-0.935755, 0.283165, -0.210192;\n",
            "\t-0.302933, -0.950581, 0.0680313\n",
            "]\n",
            "t:   0.58716 -0.381202   4.47134\n",
            ".calibration. K: 1000    0  640\n",
            "   0 1000  360\n",
            "   0    0    1\n",
            "Baseline: 0.5\n",
            "\n",
            "Local Coordinates: [0.1 0.2 0.3 0.4 0.5 0.6]\n"
          ]
        }
      ],
      "source": [
        "print(\"Original StereoCamera:\")\n",
        "stereo_camera.print()\n",
        "\n",
        "# Define a delta vector in the tangent space.\n",
        "delta = np.array([0.1, 0.2, 0.3, 0.4, 0.5, 0.6])\n",
        "\n",
        "# Retract the camera pose.\n",
        "retracted_camera = stereo_camera.retract(delta)\n",
        "print(\"\\nRetracted StereoCamera:\")\n",
        "retracted_camera.print()\n",
        "\n",
        "# Calculate the local coordinates between the original and retracted cameras.\n",
        "local_coords = stereo_camera.localCoordinates(retracted_camera)\n",
        "print(\"\\nLocal Coordinates:\", local_coords)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Additional Resources\n",
        "\n",
        "The following resources provide more detailed explanations of camera calibration and stereo vision.\n",
        "\n",
        "### Article(s)\n",
        "- [\"5.3. Cameras for Robot Vision\"](https://www.roboticsbook.org/S53_diffdrive_sensing.html) by Dr. Frank Dellaert and Dr. Seth Hutchinson\n",
        "\n",
        "### Video(s)\n",
        "- [\"Simple Stereo | Camera Calibration\"](https://youtu.be/hUVyDabn1Mg?si=3zIrPPML-H7Zu5_i) by Dr. Shree Nayar from Columbia University\n",
        "\n",
        "## Source\n",
        "- [StereoCamera.h](https://github.com/borglab/gtsam/blob/develop/gtsam/geometry/StereoCamera.h)\n",
        "- [StereoCamera.cpp](https://github.com/borglab/gtsam/blob/develop/gtsam/geometry/StereoCamera.cpp)"
      ]
    }
  ],
  "metadata": {
    "colab": {
      "provenance": []
    },
    "kernelspec": {
      "display_name": "base",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.12.10"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 0
}